#include "MultiRobotCollisionDetector.h"
#include <boost/foreach.hpp>

//#define PRINT_STATUS

MultiRobotCollisionDetector::MultiRobotCollisionDetector(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles) :
    m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles)
{
    vector<Obstacles> robot_obs; // The other robots as obstacles
#ifdef PRINT_STATUS
    std::cout << "[mr CollisionD]\tInitializing " << robot_polygons.size() << " robots";
    int robot_index = 1;

    std::cout << " [#" << robot_index << "]";
#endif
    BOOST_FOREACH(Polygon_2& robot, m_robot_polygons)
    {
#ifdef PRINT_STATUS
        std::cout << "\x08\x08" << robot_index << "]";
        ++robot_index;
#endif
        CollisionDetector* collision = new CollisionDetector(robot, m_obstacles);
        m_cols.push_back(collision);
        vector<CollisionDetector*> cols;

#ifdef PRINT_STATUS
        int index = 0;
        int max_value = robot_obs.size();
        std::cout << "  0%";
#endif
        BOOST_FOREACH(Obstacles& other, robot_obs)
        {
            CollisionDetector* collision2 = new CollisionDetector(robot, other);
            cols.push_back(collision2);
#ifdef PRINT_STATUS
            //if (index % 5 == 0)
            {
                int per = (int)(((1.0 * index) / max_value) * 100);
                if (per == 100)
                {
                    per = 99;
                }
                if (per < 10)
                {
                    std::cout << "\x08\x08" << per << "%";
                }
                else
                {
                    std::cout << "\x08\x08\x08" << per << "%";
                }
            }
#endif
        }
        m_colsRobots.push_back(cols);
#ifdef PRINT_STATUS
        std::cout << "\x08\x08\x08\x08";
#endif

        Obstacles next;
        next.push_back(robot);
        robot_obs.push_back(next);
    }
#ifdef PRINT_STATUS
    std::cout << "\x08\x08\x08\x08\x08 - Done!  \n";
#endif
}

MultiRobotCollisionDetector::~MultiRobotCollisionDetector(void)
{
    for (int index = 0; index < m_robot_num; ++index)
    {
        delete m_cols[index];
        for (int j = 0; j < m_colsRobots[index].size(); ++j)
        {
            delete m_colsRobots[index][j];
        }
    }
    m_cols.clear();
}

vector<CollisionDetector*>::const_iterator MultiRobotCollisionDetector::begin()
{
    return m_cols.cbegin();
}

vector<CollisionDetector*>::const_iterator MultiRobotCollisionDetector::end()
{
    return m_cols.cend();
}

bool MultiRobotCollisionDetector::valid_conf(vector<Conf> &pos)
{
    for(int index = 0; index < m_robot_num; ++index)
    {
        for(int j = 0; j < index; ++j)
        {
            if (!m_colsRobots[index][j]->valid_conf(pos[j]))
            {
                return false;
            }
        }
    }
    return true;
}

bool MultiRobotCollisionDetector::local_planner(vector<Conf> &start, vector<Conf> &target, double eps)
{
    if (is_colliding(start, target, eps))
        return false;

    //use collision detector to find yes/no for each robot
    for (int index = 0; index < m_robot_num; ++index)
    {
        if (!m_cols[index]->local_planner(start[index], target[index], eps))
            return false;
    }
    return true;
}

bool MultiRobotCollisionDetector::is_colliding(vector<Conf> &start, vector<Conf> &target, double eps)
{
    for(int index = 0; index < m_robot_num; ++index)
    {
        for(int j = 0; j < index; ++j)
        {
            Vector_2 vecS = start[j] - start[index];
            Vector_2 vecE = target[j] - target[index];
            Point_2 s(vecS.x(), vecS.y());
            Point_2 e(vecE.x(), vecE.y());
            if (!m_colsRobots[index][j]->local_planner(s, e, eps))
                return true;
        }
    }

    return false;
}
